/**
 * @file 	 sq680.c
 * @brief
 * @author 	 wenbo
 * @version  1.0
 * @date 	 2022-02-11
 *
 * @copyright Copyright (c) 2022  YGL
 *
 */

#include "sq680.h"
#include "stdlib.h"
#include "string.h"
#include "boardio.h"

#define _ABS(n)  (n < 0 ? (-n) : (n))
#define IS_VALID_PIN(id)    (id > 0 ? (true) : (false))
#define IS_SQ680_CHECK_OK(devId) (((devId) < g_sq680_listLen) && (g_sq680_list) && (g_sq680_list[(devId)].isInitOk))


 //不同的项目需要在应用层配置好Type_sq680_Def列表然后传入初始化
 /**************************************************************/
static uint8_t g_sq680_listLen = 0;
static Type_sq680_Def* g_sq680_list = NULL;
static const char str_mode[][10] = { "NONE", "MODE_KEY", "MODE_IO", "MODE_AUTO", "MODE_485" };
static const char str_F_Src[][15] = { "NONE", "F_SRC_KNOB", "F_src_AI1", "F_src_KEY", "F_src_485", "F_src_PLC", "F_src_PID" };
/**************************************************************/


static int xp_sq680_debug(char* type, char* fun, char* param);

bool _xp_sq680_update_mode(Type_sq680_Def* const handle);
bool _xp_sq680_update_F_src(Type_sq680_Def* const handle);
bool _xp_sq680_set_mode(Type_sq680_Def* const handle, Type_sq680_Mode_Enum mode);
bool _xp_sq680_update_acc_time(Type_sq680_Def* const handle);
bool _xp_sq680_set_acc_time(Type_sq680_Def* const handle, uint16_t val);
bool _xp_sq680_update_dec_time(Type_sq680_Def* const handle);
bool _xp_sq680_set_dec_time(Type_sq680_Def* const handle, uint16_t val);
bool _xp_sq680_update_current(Type_sq680_Def* const handle);
bool _xp_sq680_update_error_code(Type_sq680_Def* const handle);
bool _xp_sq680_set_io_default_function(Type_sq680_Def* const handle);
int xp_sq680_write_reginster(Type_sq680_Def* const handler, uint16_t addr, uint16_t* data, uint8_t len);
int xp_sq680_read_reginster(Type_sq680_Def* const handler, uint16_t addr, uint16_t read_len, uint8_t* rec_data, uint8_t* rec_len);

void memcpy_resv_word(void* des, void* const res, uint16_t size);
void xp_sq680_show(void);

//变频器列表 Demo
// static Type_sq680_Def SQ680_list[] = {

//     [0] = {

//         .isReverseDir = false,

//         .com = {
//             .slave_id = 1,
//             .port = 1,
//             .speed = 9600,
//         },

//         .accTime = 10,  // 单位0.1s
//         .decTime = 10,  // 单位0.1s

//         .gpio_STF.boardId = 1,   .gpio_STF.pinId = 0,
//         .gpio_STR.boardId = 1,   .gpio_STR.pinId = 1,
//         .gpio_S0.boardId = 1,    .gpio_S0.pinId = 2,
//         .gpio_S1.boardId = 1,    .gpio_S1.pinId = 3,
//         .gpio_S2.boardId = 1,    .gpio_S2.pinId = 4,
//     },
// };


// sq680初始化
bool xp_sq680_init(Type_sq680_Def* const pList, uint8_t listLen)
{
    int ret = 0;

    if (NULL != pList && 0 != listLen) {

        g_sq680_list = pList;
        g_sq680_listLen = listLen;

        for (uint8_t i = 0; i < g_sq680_listLen; i++) {

            g_sq680_list[i].isInitOk = false;

            ret = xp_modbus_init(g_sq680_list[i].com.port, g_sq680_list[i].com.speed, MB_PAR_NONE, 100);

            if (0 != ret) {
                println("sq680 <id:%d> modbus init fail ! ret = %d", g_sq680_list[i].com.slave_id, ret);
                return false;
            }
            else {
                bool isOK = false;

                /* 读取变频器运行模式 */
                isOK = _xp_sq680_update_mode(&g_sq680_list[i]);
                if (!isOK) { return false; }
                println("sq680 <id:%d> mode: %s", g_sq680_list[i].com.slave_id, g_sq680_list[i].mode < MODE_MAX ? (str_mode[g_sq680_list[i].mode]) : ("UNKNOW"));

                /* 读取变频器频率源 */
                isOK = _xp_sq680_update_F_src(&g_sq680_list[i]);
                if (!isOK) { return false; }
                println("sq680 <id:%d> F_src: %s", g_sq680_list[i].com.slave_id, g_sq680_list[i].F_src < F_SRC_MAX ? (str_F_Src[g_sq680_list[i].F_src]) : ("UNKNOW"));

                /* 设置变频器端子的默认功能(通用IO控制) */
                isOK = _xp_sq680_set_io_default_function(&g_sq680_list[i]);
                if (!isOK) { return false; }

                /* 设置或更新变频器加速时间 */
                if (0 != g_sq680_list[i].accTime){
                    isOK = _xp_sq680_set_acc_time(&g_sq680_list[i], g_sq680_list[i].accTime);
                    if (!isOK) { return false; }
                    println("sq680 <id:%d> set accTime: %.2fs", g_sq680_list[i].com.slave_id, g_sq680_list[i].accTime / 10.0f);
                }else{
                    isOK = _xp_sq680_update_acc_time(&g_sq680_list[i]);
                    if (!isOK) { return false; }
                    println("sq680 <id:%d> get accTime: %.2fs", g_sq680_list[i].com.slave_id, g_sq680_list[i].accTime / 10.0f);
                }

                /* 设置或更新变频器减速时间 */
                if (0 != g_sq680_list[i].accTime){
                    isOK = _xp_sq680_set_dec_time(&g_sq680_list[i], g_sq680_list[i].accTime);
                    if (!isOK) { return false; }
                    println("sq680 <id:%d> set decTime: %.2fs", g_sq680_list[i].com.slave_id, g_sq680_list[i].accTime / 10.0f);
                }else{
                    isOK = _xp_sq680_update_dec_time(&g_sq680_list[i]);
                    if (!isOK) { return false; }
                    println("sq680 <id:%d> get decTime: %.2fs", g_sq680_list[i].com.slave_id, g_sq680_list[i].accTime / 10.0f);
                }

                g_sq680_list[i].isInitOk = true;
                println("sq680 <id:%d> init success", g_sq680_list[i].com.slave_id);
            }
        }

        xp_cli_debug_add(xp_sq680_debug);
        return true;

    }
    else {
        return false;
    }
}
// sq680去初始化
bool xp_sq680_Deinit(void)
{
    bool ret = true;
    for (int i = 0; i < g_sq680_listLen; i++) {
        if (MODE_485 == g_sq680_list[i].mode) {
            if (xp_sq680_bus_run(i, 0)) {
                g_sq680_list[i].isInitOk = false;
            }
            else {
                ret = false;
            }
        }
        else {
            if (xp_sq680_io_run(i, 0)) {
                g_sq680_list[i].isInitOk = false;
            }
            else {
                ret = false;
            }
        }
    }
    return ret;
}
// sq680 读取当前模式
bool _xp_sq680_update_mode(Type_sq680_Def* const handle)
{
    int ret = xp_sq680_read_reginster(handle, ADDR_RUN_MODE, 1, handle->com.rec_data, &handle->com.rec_len);
    if (0 == ret) {
        handle->mode = (handle->com.rec_data[0] << 8) | handle->com.rec_data[1];
        return true;
    }
    return false;
}
// sq680 设置命令模式
bool _xp_sq680_set_mode(Type_sq680_Def* const handle, Type_sq680_Mode_Enum val)
{
    uint16_t date = val;
    int ret = xp_sq680_write_reginster(handle, ADDR_RUN_MODE, &date, 1);
    if (0 == ret) {
        handle->mode = val;
        return true;
    }
    return false;
}
// sq680 读取频率源
bool _xp_sq680_update_F_src(Type_sq680_Def* const handle)
{
    int ret = xp_sq680_read_reginster(handle, ADDR_FREQ_SRC, 1, handle->com.rec_data, &handle->com.rec_len);
    if (0 == ret) {
        handle->F_src = (handle->com.rec_data[0] << 8) | handle->com.rec_data[1];
        return true;
    }
    return false;
}
// sq680 设置频率源
bool _xp_sq680_set_F_src(Type_sq680_Def* const handle, Type_sq680_F_Src_Enum val)
{
    uint16_t date = val;
    int ret = xp_sq680_write_reginster(handle, ADDR_FREQ_SRC, &date, 1);
    if (0 == ret) {
        handle->F_src = val;
        return true;
    }
    return false;
}
// sq680 设置目标频率
bool _xp_sq680_set_tar_freq(Type_sq680_Def* const handle, uint16_t val)
{
    uint16_t date = val;
    int ret = xp_sq680_write_reginster(handle, ADDR_TAR_FREQ, &date, 1);
    if (0 == ret) {
        handle->tar_freq = val;
        return true;
    }
    return false;
}
// sq680 设置控制寄存器
bool _xp_sq680_set_ctrl_reg(Type_sq680_Def* const handle, Type_sq680_Ctrl_Enum val)
{
    uint16_t date = val;
    int ret = xp_sq680_write_reginster(handle, ADDR_CTRL_REG, &date, 1);
    if (0 == ret) {
        handle->ctrl = val;
        return true;
    }
    return false;
}
// sq680 更新加速时间
bool _xp_sq680_update_acc_time(Type_sq680_Def* const handle)
{
    int ret = xp_sq680_read_reginster(handle, ADDR_ACC_TIME, 1, handle->com.rec_data, &handle->com.rec_len);
    if (0 == ret) {
        handle->accTime = (handle->com.rec_data[0] << 8) | handle->com.rec_data[1];
        return true;
    }
    return false;
}
// sq680 设置加速时间
bool _xp_sq680_set_acc_time(Type_sq680_Def* const handle, uint16_t val)
{
    uint16_t date = val;
    int ret = xp_sq680_write_reginster(handle, ADDR_ACC_TIME, &date, 1);
    if (0 == ret) {
        handle->accTime = val;
        return true;
    }
    return false;
}
// sq680 更新减速时间
bool _xp_sq680_update_dec_time(Type_sq680_Def* const handle)
{
    int ret = xp_sq680_read_reginster(handle, ADDR_DEC_TIME, 1, handle->com.rec_data, &handle->com.rec_len);
    if (0 == ret) {
        handle->decTime = (handle->com.rec_data[0] << 8) | handle->com.rec_data[1];
        return true;
    }
    return false;
}
// sq680 设置减速时间
bool _xp_sq680_set_dec_time(Type_sq680_Def* const handle, uint16_t val)
{
    uint16_t date = val;
    int ret = xp_sq680_write_reginster(handle, ADDR_DEC_TIME, &date, 1);
    if (0 == ret) {
        handle->decTime = val;
        return true;
    }
    return false;
}
// sq680 读取电流
bool _xp_sq680_update_current(Type_sq680_Def* const handle)
{
    int ret = xp_sq680_read_reginster(handle, ADDR_CURRENT, 1, handle->com.rec_data, &handle->com.rec_len);
    if (0 == ret) {
        handle->current = (handle->com.rec_data[0] << 8) | handle->com.rec_data[1];
        return true;
    }
    return false;
}
// sq680 读取错误码
bool _xp_sq680_update_error_code(Type_sq680_Def* const handle)
{
    int ret = xp_sq680_read_reginster(handle, ADDR_ERROR_CODE, 1, handle->com.rec_data, &handle->com.rec_len);
    if (0 == ret) {
        handle->errCode = (handle->com.rec_data[0] << 8) | handle->com.rec_data[1];
        return true;
    }
    return false;
}
// sq680 设置段速
bool _xp_sq680_set_stage_speed(Type_sq680_Def* const handle, uint8_t stage_id, uint16_t val)
{
    int ret = xp_sq680_write_reginster(handle, ADDR_STAGE_BASE + stage_id * 5, &val, 1);
    if (0 == ret) {
        handle->stageTable[stage_id] = val;
        return true;
    }
    return false;
}
// sq680 设置端子默认功能
bool _xp_sq680_set_io_default_function(Type_sq680_Def* const handle)
{
    uint16_t val = DI_FUN_FWD;
    int ret = xp_sq680_write_reginster(handle, ADDR_DI_S1_FUN, &val, 1);
    if (0 != ret) { return false; }

    val = DI_FUN_REV;
    ret = xp_sq680_write_reginster(handle, ADDR_DI_S2_FUN, &val, 1);
    if (0 != ret) { return false; }

    val = DI_FUN_STAGE_1;
    ret = xp_sq680_write_reginster(handle, ADDR_DI_S3_FUN, &val, 1);
    if (0 != ret) { return false; }

    val = DI_FUN_STAGE_2;
    ret = xp_sq680_write_reginster(handle, ADDR_DI_S4_FUN, &val, 1);
    if (0 != ret) { return false; }

    val = DI_FUN_STAGE_3;
    ret = xp_sq680_write_reginster(handle, ADDR_DI_S5_FUN, &val, 1);
    if (0 != ret) { return false; }

    return true;
}
int xp_sq680_write_reginster(Type_sq680_Def* const handler, uint16_t addr, uint16_t* data, uint8_t len)
{
    int sta = 0;
    MB_data modbus;
    uint8_t try_times = 3;

    modbus.slave_id = handler->com.slave_id;
    modbus.mb_fun = mb_single_w;
    modbus.start_addr = addr;
    modbus.wBuf = data;
    modbus.rwLen = len;
    modbus.resBuf = handler->com.rec_data;
    modbus.resLen = &handler->com.rec_len;
    while (try_times--) {
        if (0 == xp_modbus_ask(handler->com.port, &modbus)) return 0;
    }

    return -1;
}

int xp_sq680_read_reginster(Type_sq680_Def* const handler, uint16_t addr, uint16_t read_len, uint8_t* rec_data, uint8_t* rec_len)
{
    int sta = 0;
    MB_data modbus;
    uint8_t try_times = 3;

    modbus.slave_id = handler->com.slave_id;
    modbus.mb_fun = mb_hold_r;
    modbus.start_addr = addr;
    modbus.rwLen = read_len;
    modbus.resBuf = rec_data;
    modbus.resLen = rec_len;
    while (try_times--) {
        if (0 == xp_modbus_ask(handler->com.port, &modbus)) return 0;
    }
    println("sq680 read fail !!!");
    return -1;
}
//带大小端转换的memcpy  __REVSH
static void memcpy_resv_word(void* des, void* const res, uint16_t size)
{
    for (uint16_t i = 0; i < size; i += sizeof(uint32_t)) {
        *((uint8_t*)des + i + 0) = *((uint8_t*)res + i + 3);
        *((uint8_t*)des + i + 1) = *((uint8_t*)res + i + 2);
        *((uint8_t*)des + i + 2) = *((uint8_t*)res + i + 1);
        *((uint8_t*)des + i + 3) = *((uint8_t*)res + i + 0);
    }
}
//输出各参数
void xp_sq680_show(void)
{
    for (uint8_t i = 0; i < g_sq680_listLen; i++) {
        bool isOK = false;
        /* 读取变频器运行模式 */
        isOK = _xp_sq680_update_mode(&g_sq680_list[i]);
        if (isOK) {
            println("sq680 <id:%d> mode: %s", g_sq680_list[i].com.slave_id, g_sq680_list[i].mode < MODE_MAX ? (str_mode[g_sq680_list[i].mode]) : ("UNKNOW"));
        }
        else {
            println("sq680 <id:%d> mode update failed");
        }

        /* 读取变频器频率源 */
        isOK = _xp_sq680_update_F_src(&g_sq680_list[i]);
        if (isOK) {
            println("sq680 <id:%d> F_src: %s", g_sq680_list[i].com.slave_id, g_sq680_list[i].F_src < F_SRC_MAX ? (str_F_Src[g_sq680_list[i].F_src]) : ("UNKNOW"));
        }
        else {
            println("sq680 <id:%d> F_src update failed");
        }

        // /* 读取变频器当前电流 */
        // isOK = _xp_sq680_update_current(&g_sq680_list[i]);
        // if (isOK) {
        //     println("sq680 <id:%d> current: %s", g_sq680_list[i].com.slave_id, g_sq680_list[i].current);
        // }else{
        //     println("sq680 <id:%d> current update failed");
        // }

        /* 读取变频器当前加速时间 */
        isOK = _xp_sq680_update_acc_time(&g_sq680_list[i]);
        if (isOK) {
            println("sq680 <id:%d> acc time: %d(%.2fs)", g_sq680_list[i].com.slave_id, g_sq680_list[i].accTime, g_sq680_list[i].accTime / 10.0f);
        }

        /* 读取变频器当前减速时间 */
        isOK = _xp_sq680_update_dec_time(&g_sq680_list[i]);
        if (isOK) {
            println("sq680 <id:%d> dec time: %d(%.2fs)", g_sq680_list[i].com.slave_id, g_sq680_list[i].decTime, g_sq680_list[i].decTime / 10.0f);
        }

        /* 读取变频器当前错误码 */
        isOK = _xp_sq680_update_error_code(&g_sq680_list[i]);
        if (isOK) {
            println("sq680 <id:%d> error_code: %04X", g_sq680_list[i].com.slave_id, g_sq680_list[i].errCode);
        }
        else {
            println("sq680 <id:%d> error_code update failed");
        }
    }
}



//外部接口
/********************************************************************************************/
// IO输出底层接口
void xp_sq680_write_pin(const uint8_t board_id, const uint8_t pin, const bool isEnable)
{
    if (IS_VALID_PIN(board_id)) {    //通过板ID是否为0验证此IO是否实际使用(板ID从1开始)
        xp_io_write_pin(board_id, pin, isEnable ? (0) : (1));
    }
}
// 通过变频器索引ID获取电流
bool xp_sq680_get_current(int devId, int16_t* pResult)
{
    if (IS_SQ680_CHECK_OK(devId) && pResult) {
        if (_xp_sq680_update_current(&g_sq680_list[devId])) {
            *pResult = g_sq680_list[devId].current;
            return true;
        }
        else {
            return false;
        }
    }
    return false;
}
// 通过变频器索引ID获取错误码
bool xp_sq680_get_error_code(int devId, uint32_t* pResult)
{
    if (IS_SQ680_CHECK_OK(devId) && pResult) {
        if (_xp_sq680_update_error_code(&g_sq680_list[devId])) {
            *pResult = g_sq680_list[devId].errCode;
            return true;
        }
        else {
            return false;
        }
    }
    return false;
}
// 通过变频器索引ID使用总线运转变频器
bool xp_sq680_bus_run(int devId, int freq)
{
    if (IS_SQ680_CHECK_OK(devId)) {
        bool ret = false;

        if (g_sq680_list[devId].isReverseDir){
            freq = -freq;
        }

        /* 设置运行模式为RS485（切换只能在停机状态时有效） */
        if (MODE_485 != g_sq680_list[devId].mode) {   
            ret = _xp_sq680_set_mode(&g_sq680_list[devId], MODE_485);
            // printf("sq680 slave_id :%d set to 485 mode[%d] %s\r\n", g_sq680_list[devId].com.slave_id, MODE_485, ret ? ("OK") : ("failed"));
            if (!ret) { return false; }
        }

        /* 设置频率源为modbus（切换只能在停机状态时有效） */
        if (F_SRC_MODBUS != g_sq680_list[devId].F_src) {
            ret = _xp_sq680_set_F_src(&g_sq680_list[devId], F_SRC_MODBUS);
            // printf("sq680 slave_id :%d set to F_SRC_MODBUS[%d] %s\r\n", g_sq680_list[devId].com.slave_id, F_SRC_MODBUS, ret ? ("OK") : ("failed"));
            if (!ret) { return false; }
        }

        /* 设置目标频率 */
        ret = _xp_sq680_set_tar_freq(&g_sq680_list[devId], (freq < 0) ? (-freq) : (freq));
        // printf("sq680 slave_id :%d set tar freq %d %s\r\n", g_sq680_list[devId].com.slave_id, freq, ret ? ("OK") : ("failed"));

        /* 通过控制寄存器设置正反转 */
        if (freq > 0) {
            ret = _xp_sq680_set_ctrl_reg(&g_sq680_list[devId], (freq < 0) ? (CTRL_REV) : (CTRL_FWD));
            // printf("sq680 slave_id :%d set to FWD %s\r\n", g_sq680_list[devId].com.slave_id, ret ? ("OK") : ("failed"));
        }else if (freq < 0) {
            ret = _xp_sq680_set_ctrl_reg(&g_sq680_list[devId], (freq < 0) ? (CTRL_REV) : (CTRL_FWD));
            // printf("sq680 slave_id :%d set to REV %s\r\n", g_sq680_list[devId].com.slave_id, ret ? ("OK") : ("failed"));
        }else {
            ret = _xp_sq680_set_ctrl_reg(&g_sq680_list[devId], CTRL_SLOW_STOP);
            // printf("sq680 slave_id :%d set to stop %s\r\n", g_sq680_list[devId].com.slave_id, ret ? ("OK") : ("failed"));
        }
        return ret;
    }
    return false;
}
// 通过变频器索引ID及段速索引ID设置相应段速值
bool xp_sq680_set_stage_speed(int devId, uint8_t stage_id, uint16_t val)
{
    if (IS_SQ680_CHECK_OK(devId) && stage_id < sizeof(g_sq680_list[devId].stageTable) / sizeof(g_sq680_list[devId].stageTable[0])) {
        return _xp_sq680_set_stage_speed(&g_sq680_list[devId], stage_id, val);
    }
    return false;
}
// 通过变频器索引ID设置加速时间
bool xp_sq680_set_acc_time(int devId, uint16_t val)
{
    if (IS_SQ680_CHECK_OK(devId)) {
        return _xp_sq680_set_acc_time(&g_sq680_list[devId], val);
    }
    return false;
}
// 通过变频器索引ID设置减速时间
bool xp_sq680_set_dec_time(int devId, uint16_t val)
{
    if (IS_SQ680_CHECK_OK(devId)) {
        return _xp_sq680_set_dec_time(&g_sq680_list[devId], val);
    }
    return false;
}
// 通过变频器索引ID恢复故障
bool xp_sq680_clear_error(int devId)
{
    if (IS_SQ680_CHECK_OK(devId)) {
        return _xp_sq680_set_ctrl_reg(&g_sq680_list[devId], CTRL_RECOVER);
    }
    return false;
}
// 通过变频器索引ID使用IO运转变频器
bool xp_sq680_io_run(uint8_t devId, int speed)
{
    if (IS_SQ680_CHECK_OK(devId) && _ABS((int)speed) <= 7) {
        bool ret = false;

        if (g_sq680_list[devId].isReverseDir){
            speed = -speed;
        }

        /* 设置运行模式为外部端子（切换只能在停机状态时有效） */
        if (MODE_IO != g_sq680_list[devId].mode) { 
            ret = _xp_sq680_set_mode(&g_sq680_list[devId], MODE_IO);
            printf("sq680 slave_id :%d set to IO mode[%d] %s\r\n", g_sq680_list[devId].com.slave_id, MODE_IO, ret ? ("OK") : ("failed"));
            if (!ret) { return false; }
        }

        if (speed > 0) {         //正转, X1端子（FWD）短路到COM X2端子（REV）释放
            xp_sq680_write_pin(g_sq680_list[devId].gpio_STF.boardId, g_sq680_list[devId].gpio_STF.pinId, true);
            xp_sq680_write_pin(g_sq680_list[devId].gpio_STR.boardId, g_sq680_list[devId].gpio_STR.pinId, false);
        }
        else if (speed < 0) {   //反转
            xp_sq680_write_pin(g_sq680_list[devId].gpio_STF.boardId, g_sq680_list[devId].gpio_STF.pinId, false);
            xp_sq680_write_pin(g_sq680_list[devId].gpio_STR.boardId, g_sq680_list[devId].gpio_STR.pinId, true);
        }
        else {                  //停止 speed == 0
            xp_sq680_write_pin(g_sq680_list[devId].gpio_STF.boardId, g_sq680_list[devId].gpio_STF.pinId, false);
            xp_sq680_write_pin(g_sq680_list[devId].gpio_STR.boardId, g_sq680_list[devId].gpio_STR.pinId, false);
        }

        speed = _ABS((int)speed);
        /* 对于sq680 X1 X2 X3 = 000 时为主段速,但没有主段速相关资料，实际测试000时变频器停止，因此速度范围为0~7 */

        xp_sq680_write_pin(g_sq680_list[devId].gpio_S0.boardId, g_sq680_list[devId].gpio_S0.pinId, (speed & 0x01) ? (true) : (false));
        xp_sq680_write_pin(g_sq680_list[devId].gpio_S1.boardId, g_sq680_list[devId].gpio_S1.pinId, (speed & 0x02) ? (true) : (false));
        xp_sq680_write_pin(g_sq680_list[devId].gpio_S2.boardId, g_sq680_list[devId].gpio_S2.pinId, (speed & 0x04) ? (true) : (false));

        return true;
    }
    return false;
}



//调试接口
/********************************************************************************************/
static int xp_sq680_debug(char* type, char* fun, char* param)
{
    //本调试接口仅支持 "sq680_" 起始的命令
    if (NULL == strstr(type, "sq680_")) {
        return 0;
    }
    else {
        type += strlen("sq680_");
    }
    //xp sq680_show  输出相关信息
    if (strcasecmp(type, "show") == 0) {
        xp_sq680_show();
    }
    else if (strcasecmp(type, "io_run") == 0) {
        println("Use io run stage speed %d(-7 ~ 7) %s\r\n", atoi(param), xp_sq680_io_run(atoi(fun), atoi(param)) == true ? ("success") : ("failed"));
    }
    else if (strcasecmp(type, "bus_run") == 0) {
        println("Use bus run %.2fHz %s\r\n", atoi(param) / 100.0f, xp_sq680_bus_run(atoi(fun), atoi(param)) == true ? ("success") : ("failed"));
    }
    else if (strcasecmp(type, "set_acc") == 0) {
        println("set acc time %.2fs %s\r\n", atoi(param) / 10.0f, xp_sq680_set_acc_time(atoi(fun), atoi(param)) == true ? ("success") : ("failed"));
    }
    else if (strcasecmp(type, "set_dec") == 0) {
        println("set dec time %.2fs %s\r\n", atoi(param) / 10.0f, xp_sq680_set_dec_time(atoi(fun), atoi(param)) == true ? ("success") : ("failed"));
    }
    else {
        return 0;
    }
    return 1;
}